/***********************************************************************
 * $Id:: BLDC.c 3871 2010-07-16 11:50:22Z gerritdewaard                $
 *
 * Project: LPC1100 BLDC Motor Control AN
 *
 * Description:
 *
 *
 *
 ***********************************************************************
 * Software that is described herein is for illustrative purposes only
 * which provides customers with programming information regarding the
 * products. This software is supplied "AS IS" without any warranties.
 * NXP Semiconductors assumes no responsibility or liability for the
 * use of the software, conveys no license or title under any patent,
 * copyright, or mask work right to the product. NXP Semiconductors
 * reserves the right to make changes in the software without
 * notification. NXP Semiconductors also make no representation or
 * warranty that such application will be suitable for the specified
 * use without further testing or modification.
 **********************************************************************/
#include "application.h"

#define MAX_ERROR 	600
#define MAX_RPM		4000

/******************************************************************************
** Function name:		PID_calc_cntr
**
** Descriptions:
**
** parameters:
** Returned value:
**
******************************************************************************/
//#define NEW_PID
void vPID_RPM( MOTOR_TypeDef *ptr )
{
#ifdef NEW_PID
	int32_t output, error, Pout = 0, Dout = 0, Iout = 0;

	/* Calculate the error from setpoint and actual RPM  */
    error = ptr->sp - ptr->RPM;

	if (error > MAX_ERROR) error = MAX_ERROR;

    /* Calculate and update the Integrated Error */
    Iout = (((ptr->Ki * error * 10) + ptr->IntError));
    ptr->IntError = Iout;
    //Iout = (((ptr->Ki * ptr->LastError)));

    /* Calculate the proportional term  */
    Pout = ((ptr->Kp * error));

    /* Calculate the derivative term */
    Dout = (error - ptr->LastError) * ptr->Kd;

    /* Calculate the output from the Pout, Iout, Dout  */
    output = Pout + (Iout/10) + Dout;

    /* Save the error */
    ptr->LastError = error;

    /*  */
    output = ((output*100)/MAX_RPM)*ptr->max_mv;
    ptr->mv = output/100 ;

//    /* Clip the output to 0 */
//    if (output < 0) output = 0;
//
//    /* Clip output to MAX PERIOD */
//    if (output > ptr->max_mv) output = MOTOR_MAX_PERIOD;
//
//    /* Set the PID output value in the structure */
//   	ptr->mv = output;

#else

    double temp;

    //ptr->err[0] = ((int32_t)ptr->sp - ptr->RPM);
    ptr->err[0] = ((int32_t)ptr->sp - ptr->pv);

    temp = ((ptr->err[0]*ptr->Kp)/10) + ((ptr->err[1]*ptr->Ki)/1000); //ptr->U    + (( ptr->K1 * ptr->err[0] ) + ( ptr->K2 * ptr->err[1] ) + ( ptr->K3 * ptr->err[2] ));

    if(temp<0)
    {
        ptr->mv=0;
    }
    else
    {
        ptr->mv=(uint32_t)temp;
    }

	if((ptr->mv < ptr->max_mv) && (ptr->mv > 0))
	{
		ptr->err[1] += ptr->err[0];//(ptr->err[0]/(ptr->RPM/6));
	}


#endif

}

